//
// Created by ubuntu on 2024/1/4.
//

#ifndef LASER_LINE_FITTER_SUBSCRIBER_SCAN_H
#define LASER_LINE_FITTER_SUBSCRIBER_SCAN_H

#include <deque>
#include <mutex>
#include <thread>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2_ros/buffer.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include "split_and_merge.h"
#include "seed_area_growth.h"
#include "line.h"

namespace line_extraction
{

class SubcriberScan
{
public:
    SubcriberScan(ros::NodeHandle &nh, ros::NodeHandle &nh_local, std::string topic_name, size_t buff_size, std::string frame_ID);

    SubcriberScan() = default;

    ~SubcriberScan();

    void startRun();

private:
    void loadParameters();

    void msg_callback(const sensor_msgs::LaserScan::ConstPtr &scan_msg);

    void cacheData(const sensor_msgs::LaserScan::ConstPtr &scan_msg);

    void populateMarkerMsg(const std::vector<gline> &lines);

    Params params_;
    CSdata cs_data_;      // 数据形势
    SplitAndMerge split_and_merge_;
    SeedAreaGrowth seed_area_growth_;
    ros::NodeHandle nh_;
    ros::NodeHandle nh_local_;    // Line extraction
    ros::Subscriber subscriber_;
    ros::Publisher marker_publisher_;
    std::deque<Rangedata> new_scan_data_;
    geometry_msgs::TransformStamped transform;
    double roll, pitch, yaw;
    std::string frame_id_;
    std::mutex buff_mutex_;
    bool is_split_and_merge = false;
    bool is_calc_error = false;
    bool data_cached_; // true after first scan used to cache data
};

}

#endif //LASER_LINE_FITTER_SUBSCRIBER_SCAN_H
